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Abstract — Sensor fusion of multiple sources plays an im- 
portant role in vehicular systems to achieve refined target 
position and velocity estimates. In this article, we address the 
general registration problem, which is a key module for a fusion 
system to accurately correct systematic errors of sensors. A fast 
maximum a posteriori (FMAP) algorithm for joint registration- 
tracking (JRT) is presented. The algorithm uses a recursive 
two-step optimization that involves orthogonal factorization 
to ensure numerically stability. Statistical efficiency analysis 
based on Cramer-Rao lower bound theory is presented to show 
asymptotical optimality of FMAR Also, Givens rotation is used 
to derive a fast implementation with complexity 0(n) with n 
the number of tracked targets. Simulations and experiments 
are presented to demonstrate the promise and effectiveness of 
FMAR 



I. INTRODUCTION 

Recently, active safety driver assistance (ASDA) systems 
such as adaptive cruise control (ACC) and pre-crash sens- 
ing (PCS) systems (5) have drawn considerable attention 
in intelligent transportation systems (ITS) community. To 
obtain the necessary information of surround vehicles for 
ASDA systems, multiple sensors including active radars, 
lidars, and passive cameras are mounted on vehicles. Those 
sensor systems in a vehicular system are typically calibrated 
manually. However, sensor orientation and signal output may 
drift during the life of the sensor, such that the orientation of 
the sensor relative to the vehicular frame is changed. When 
the sensor orientation drifts, measurements become skewed 
relative to the vehicle fl8l . When there are multiple sensors, 
this concern is further complicated. It is thus desirable to 
have sensor systems that automatically align sensor output 
to the vehicular frame 0], J2]. 

Two categories of approaches have been attempted to 
address the registration problem. The first category decou- 
ples tracking and registration into separate problems. In 
0, OH, El, EU, El, filters are designed to estimate 
the sensor biases by minimizing the discrepancy between 
measurements and associated fused target estimates, from 
a separated tracking module. However, these methods are 
not optimal in term of Cramer-Rao bounds ||251 . In the 
second category, the approaches jointly solve for target 
tracking and sensor registration. For example, llT9l - [E2l . 
Il26l have applied extended Kalman filtering (EKF) to an 
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augmented state vector combining the target variables and 
sensor system errors in aerospace applications. In []6] a 
fixed-lag smoothing framework is employed to estimate the 
augmented state vector to deal with communication jitters 
in networked sensors. Unscented Kalman filter (UKF) in 
lfl4l and expectation-maximization based interacting multiple 
model (IMM) in fTTI have been applied to the augmented 
state vector in vehicle-to-vehicle cooperative driving systems. 
In l23l the particle filter outperforms EKF at the cost of more 
demanding computations in state estimation for industrial 
systems. However, there are a few difficulties with these 
approaches. Since the registration parameters are constant, 
the error model of state-space is degenerated. This not only 
makes the estimation problem larger-leading to higher com- 
putational cost (complexity 0(n 3 ) with n denoting number of 
targets), but also results degenerated covariance matrices for 
the process noise vectors due to numerical instability inherent 
to EKF, UKF, and the variants. 

In this article, we propose a fast maximum a posteriori 
(FMAP) registration algorithm to tackle the problems. We 
combine all the measurement equations and process equa- 
tions to form a linearized state-space model. The registration 
and target track estimates are obtained by maximizing a 
posterior function in the state space. The performance of 
FMAP estimates is examined using Cramer-Rao lower bound 
theory. By exploiting the sparsity of the Cholesky factor of 
information matrix, a fast implementation whose complexity 
scales linearly with the numbers of targets and measurements 
is derived. 

The rest of this article is organized as follows. Section Ullis 
devoted to the algorithm derivation. An illustrative example is 
given in Section [ill] The results of simulation and experiment 
are presented in Sections |IV] and [V] respectively. Finally we 
give concluding remarks in Section [VI] 

II. Algorithm Derivation 

In this section, we address the computational issues in us- 
ing an EKF or UKF to solve joint tracking-registration (JTR). 
Fig. Q] shows the results of a joint problem with ten tracks 
and six registration parameters. The normalized covariance 
of the joint state from EKF is visualized in Fig. [TJ a )- Dark 
entries indicate strong correlations. It is clear that not only 
the tracks x and the registration a are correlated but also each 



pair of tracks in x is mutually correlated. The checkerboard 
appearance of the joint covariance matrix reveals this fact. 
Therefore, the approximation that ignores the off-diagonal 
correlated entries ll24l . 11271 is not asymptotical optimal. 

A key insight that motivates the proposed approach is 
shown in Fig.[TJb). Shown there is the Cholesky factoiQof the 
inverse covariance matrix (also known as information matrix) 
normalized like the correlation matrix. Entries in this matrix 
can be regarded as constraints, or connections, between the 
locations of targets and registration parameters. The darker 
an entry is in the display, the stronger the connection is. 
As this illustration suggests, the Cholesky factor R not only 
appears sparse but also is nicely structured. The matrix is only 
dominated by the entries within a track, or the entries between 
a track and the registration parameters. The proposed FMAP 
algorithm exploits and maintains this structure throughout the 
calculation. In addition, storing a sparse factor matrix requires 
linear space. More importantly, updates can be performed in 
linear time with regard to the number of tracks in the system. 

The sparsity of information matrix has been widely used 
to derive fast implementations of robotic Simultaneous Lo- 
calization and Mapping (SLAM) l3l. l9l. ifPHl. lfl6l. ATI. In 
those approaches the authors insightfully observed that the 
resulting information matrix is sparse if the measurements 
involve only "local" variables. However the sparsity is de- 
stroyed in time-propagation steps where old robotic poses 
are removed from the state representation by marginaliza- 
tion. Approximations (e.g., JU, |[T6l ) are needed to enforce 
sparsity during the marginalization. 

Although inspired by SLAM information filters, FMAP is 
different in the following aspects: 

« JTR and SLAM are different problems despite the 
similarity between their system dynamics equations. 
The number of stationary landmarks dominates the time 

'The Cholesky factor R of a matrix P is defined as P = 11*11. A semi- 
positive definite matrix can be decomposed into its Cholesky factor. 




mation matrix R = 



as shown in Fig. Q] 



Fig. 1. Typical snapshot of correlation matrices for JTR with 10 
tracks x and six registration parameters a. (a) a correlation matrix P 
of EKF (normalized), (b) Normalized Cholesky factor R of inverse 
covariance 



complexity in the SLAM case. On the other hand, the 
number of tracked targets that are in stochastic motion 
determines the time complexity in the JTR case. 
Unlike SLAM, FMAP needs no approximation to en- 
force the sparsity in the measurement update and time 
propagation, as illustrated in Fig. [T|(b). 
FMAP recursively computes Cholesky factor of infor- 

Rx Rxa 
Ra 

(b), contrasting the fact that information matrix is com- 
puted in the SLAM case. In addition, marginalization of 
old tracked targets x 1 does not destroy the sparsity of 
R. 

Throughout this article italic upper and lower case letters 
are used to denote matrices and vectors, respectively. A 
Gaussian distribution is denoted by information array @. 
For example, a multivariate x with density function N(x, Q) 

is denoted as p(x) oc e(~~ ~ ~^ or the information array 
[R, z] in short, where z = Rx and Q = R~ 1 R~ T . 

A. Joint State Space 

The setting for the JTR problem is that a vehicle, equipped 
with multiple sensors with unknown or partially unknown 
registration, moves through an environment containing a 
population of objects. The sensors can take measurements 
of the relative position and velocity between any individual 
object and the vehicle. 

The objective here is to derive a joint dynamics model 
where n tracks and registration parameters from k sensors 
are stacked into one large state vector as below: 

_ f T T T T T 1 T 

s — y x 1 x 2 x n a 1 .... a k j 

where the z'-th target track Xi, (i = 1, . . . n) comprises a set 
of parameters, e.g., position, velocity, and acceleration, and 
the registration parameters for y'-th sensor aj, (j = 1, . . . , k) 
comprises of location error, an azimuth alignment error, and 
range offset. 

The system dynamics equation for the state is expressed 

as: 

s(t + l) = f(s(t),w(t)) (1) 

where the function relates the state at time t to the state 
at time t+1; and where terms w are vectors of zero-mean 
noise random variables that are assumed to have nonsingular 
covariance matrices. 

The measurement process can be modeled symbolically as 
a function of target track (x), and registration parameter (a), 
such as 

o(t) = h(x(t),a(t))+v(t) (2) 

where o(t) and v(t) denote the measurements and the additive 
noise vectors at time instant t, respectively. 



B. Measurement update 

The FMAP algorithm works with the posterior density 
function p(s(t) | O(o : t)), where O(o : t) denotes a series of 
measurements {o(0), ...,o(t)} from the sensors. 

Using the Bayes rule, we obtain the posterior function as 



P(s(t) | o (0:t) ) = p(s(t) | o (0:t -i),o(i)) 

= Cl(t)p(o(t) | 0(0:(-1),S(*))P(SW I °(0:t-l)) 



(3) 



with ci(t) denotes the normalization factor. Typically, we 
will assume that measurements at time t depend only on the 
current state 

and © can be written as 



P(s(t) | O( 0:t )) = Ci{t)p{o{t) | s(t))p(s(t) I O( :t_l 



(4) 



Assuming the density functions are normally distributed, 
the prior density functional p(s \ oro-.t-i)) can be expressed 
by information array [R, z], i.e., 



P(S | O(0:t_l)) 
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(5) 



where \R\ is the determinant of i?, and AT S is the dimension 
of s. 

Linearizing (f2]i using Taylor expansion in the neighbor- 
hood [x*,a*], produces: 



C x x + C a a + u 1 + v 



(6) 



with mi = h* — C x x* - C a a*, h* = h(x*,a*), and 
Jacobian matrices C x and C a - Without loss of generality, the 
covariance matrix of v is assumed to be an identity matri:x0- 
Thus, the measurement density function can be written as 



p(o | s) 



exp( 



(2tt)^/2 
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(7) 



with N being the dimension of o. 

The negative logarithm of © is given by 

J t = - logp(s | o (0:t) ) 

= - logp(o | s) - logp(s | o (0:t _i)) - log(ci) 



(8) 



"In Bayesian filtering, the state variables are designed such that they 
contain all information gathered from the past measurements. Thus s(t) 
is a sufficient statistics of the past measurements O(o : t— l)- 

3 Unless it is necessary, we will not include time such as (t) in all the 
following equations. 

4 If not, the noise term v in (6) can be transformed to a random vector 
with identity covariance matrix. Let cov{u} = R v denote the covariance 
matrix of the measurement model. Multiplying both sides of ((6) by L v , the 
square root information matrix of R v , results in a measurement equation 
with an identity covariance matrix. 



Plugging in Eqs. (f5]) and (0, (O becomes 
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C2 (9) 



with C2 denoting terms not depending on (x,a). 

The principle of the maximum a posteriori (MAP) estima- 
tion is to maximize the posterior function with respect to the 
unknown variables (x,a). Clearly, the maximization process 
is equivalent to minimization of the squared norm in (0. 
Ignoring the constant term, the right hand side (RHS) of (0 
can be written as a matrix X expressed as: 



X = 



R, 




1? 
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(10) 



X can be turned into an upper triangular matrix by applying 
an orthogonal transformation T: 



TX = 



where e is the residual that reflects the discrepancy between 
the model and measurement. Applying orthogonal T to the 
quadratic term in (0, produces^ 
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(12) 



+ C 3 



with C3 = C2 + 5 1 1 e 1 1 2 denoting the constant term with respect 
to variables (x, a). 

Because J t is the negative logarithm of p(s \ O( 0: t))> the 
posterior density can be written as 
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P(S I °(0:t)) X e_ 

or in the information-array form 
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(13) 



(14) 



Since the estimate of the state variable s can be written as 
J = Rr l z, ( fl4b allows us to solve the estimates of the track 
variables x and registration parameters a by back-substitution 
using R and right hand side z, i.e., 



(15) 
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The least-squares \\Rs 



is invariant under an orthogonal transfor- 



mation T, i.e., \\T(Rs-z)\\ 2 = (Rs-zfT^iRs-z) = {Rs-zfiRs- 
z) = \\Rs - z\\ 2 . 



C. Time propagation 

The prior density p(s(t + 1) | O( :t)) at ^ me ^ + 1 can 
be inferred from the system dynamics and posterior function 
p( s I °(0:t)) at ti me t. Assuming registration is time-invariant 
the linear approximation of the system dynamics in (Q3 in the 
neighborhood [s*,w*] can be expressed as, 



r x{t + i) i _ r $ x o i r x i 

[ a(t + l) J " [ / J [ a J 
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(16) 



where $ x and G x are Jacobian matrices; and the nonlinear 
term U2 = f(s*,w*) — & x x* — G x w*. 

If variables s and w are denoted by the information arrays 
in < f]~4T > and [R w , z w ], respectively, the joint density function 
given the measurements O( 0: t) can be expressed in terms of 
x(t + 1) and a(t + 1) as| 



p(s(t + l),w | o ( o:t)) oc e" 



(17) 
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The quadratic exponential term in ([PTt can be denoted as 
matrix T, expressed as 



"12 



Y = A b 



(18) 



The matrix Y can be turned into a triangular matrix through 
an orthogonal transformation T, i.e., 



TY = [ A b] 



(19) 



with A = 



R wa (t + l) 

Rxa(t + 1) 

R a (t+1) 



and b 



R w (t + l) R wx {t + 1) 

R x (t + 1) 



5„(t + 1) 

Z x (t + 1) 
Za(t+1) 

Given the measurements O(o : t), the prior function p(s(t 
1) I °(0:t)) can t> e produced by marginalization on variable 
w. Applying Lemma 1631 in Appendix A, we obtain 

||fl(t + l)s(t + 1) - z(t + 1)|| 2 
p(s(t + 1) I o (0:t) ) = c 4 oxp -1! L 
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with C4 being the normalization factor. 

Therefore, we obtain the updated prior information array 
R(t + 1), z(t + 1) at time t + 1 as: 



R x (t + 1) R xa {t + 1) z x {t + l) 

Ra(t+l) ~Z a {t+l) 



"Detailed derivation is shown in Lemma Rx2| in Appendix A. 



(20) 



The derivation leading to ( l20b illustrates the purposes of using 
the information array [R, z], which is recursively updated to 
[R(t + 1), z(t + 1)] at time instant t + 1. 

One can verify (c.f., J4] VI. 3]) that the covariance ma- 
trix P(t + 1) = $P$ T + GQG T where P(t + 1) = 
fi- 1 ^ + l)fi- T (i + 1), P = R- 1 R- T , Q = R-}R- T , 



G = [G T X ] and * = 

s(t+l) = $s + Gw + u where s(t- 
w denotes the mean of w, and u - 



In addition, 





I 

1) = R- r {t+l)z{t+l), 

\ uT r. 



D. Algorithm 

Fig.[2]illustrates the flow chart of FMAP. The algorithm is 
started upon reception of sensor data, and the state variables 
of every track and registration parameters are initialized 
using zero-mean noninformative distributions, respectively. 
A data association module matches the sensor data with 
the predicted location of targets. The measurement update 
module combines the previous estimation (i.e., prior) and new 
data (i.e., matched measurement-track pairs), and updates 
target estimation and registration. FMAP checks whether 
the discrepancy between measurements and predictions (in- 
novation error) is larger than a threshold T. A change of 
registration parameters is detected when the threshold is 
surpassed, and the prior of the sensor registration is reset 
to the noninformative distribution. This is typically occurred 
when the sensor's pose is significantly moved. The time 
propagation module predicts the target and registration in 
the next time instant based on the dynamics model ( fl~6l ). 

The complete specification of the proposed algorithm is 
given in Algorithm 1. Note that the prior [i?o,-So] at time 
is initialized as Rq = el and zq = where e is a small 
positive number, and / is an identity matrix of appropriate 
dimension. 

Although static registration a is assumed in the derivation 
(i.e., Eq. (fToTl), the case in which a in stochastic motion can 
be easily accommodated. As shown in Fig. [9] the step change 
of a can be easily detected by applying threshold checking 
to innovation error curve. Once a change is detected, we 
set the registration prior to the noninformative distribution, 
i.e., z a = and R a = el. This forces FMAP to forget 
all past information regarding registration and to trigger a 
new estimation for a. Since a rarely occurs and sufficient 
duration exists between two changes, as shown in Fig. [9] we 
can approximate the dynamics of sensor by a piecewise static 
time propagation model. 

E. Statistical Efficiency Analysis 

The Cramer-Rao lower bound (CRLB) is a measure of 
statistical efficiency of an estimator. Let p(o( 0: t),s) be the 
joint probability density of the state (parameters) s at time 
instant t and the measured data O( 0:t ), and let g(o( 0: t)) be a 
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Fig. 2. Flow chart of the fast maximum a posteriori (FMAP) 
algorithm. 

Algorithm 1: FMAP update 

Require: Given prior at instant t (i.e., previous results and its 
uncertainty measure) expressed as information array [7?, z] 
and measurements o; the system dynamical equation (fT} 
and measurement equation ((2). 

Ensure: The updated estimate of s, expressed by s 

1: Compute C x , C a , and Ui. 

2: Plug prior [R, z\\ sensor measurement matrices C x and C a \ 

and vectors iti and o into matrix X (c.f., J 1 Ob > - 
3: Factorize X (c.f., Algorithm 2). 

4: Derive the posterior density information array [R, z\ as 

shown in J 14b (Measurement update). 
5: Compute the update of tracking and registration as s = 

R^z (c.f.,{[5]>)- 
6: Compute <& x , G x , and U2- 

7: Plug the posterior information array [R, z], R w , <f> x , and 

G x into Y. 
8: Factorize Y (c.f., Algorithm 3). 

9: Derive prior information array [R(t + 1), z(t + 1)] for time 
t + 1 (c.f., d20t), which can be utilized when the new sensor 
measurements are available (Time propagation). 



function of an estimate of s. The CRLB for the estimation 
error has the form 

P ee E{[g(o m ) - s}[g(o m ) - s]*} > J t (21) 

where J t is the Fisher information matrix with the elements 



J, 



8 2 log p(O(0:t),s) 

dsids-j 



Proposition 2.1: Considering the system defined by (Q]i 
and (O, the Fisher information matrix of the system J t at 
time t is identical to [.R(i)]'[-R], with R(t) defined in ( fT4b . 

Proof: The joint probability density can be derived 
from the equality p(o {0 . t] ,s) = p(s\o {0 ., t) )p(o {0:t} ). Since 
P(°(0:t)) is a function of measured data, not depending on 
the state s; therefore, we have J t = E{A logp(s|o(o : t)} 
where A denotes the second-order partial derivative, i.e., 

AL(s) = ^r^r*- Plugging (O in, the logarithm of 
the posterior function \ogp(s\o(o-.t)) reads 



Rs 



-logp(s|o (0 . t) ) = c 



where Co denotes a constant independent of s. Then the Fisher 
information matrix J t reads J t = [i?(t)]*[i?]. ■ 
If s is estimated by g(s) = E(s|o( . t )) = R~ Y z, then (fJT)) 
is satisfied with equality. Therefore the FMAP algorithm is 
optimal in the sense of CRLB. 

F. Fast Implementation 

We have observed the FMAP algorithm comprises two 
factorization operations outlined in Eqs. (Q~T) and ([T9i l, and 
back substitution operation (1151) , However, directly applying 
matrix factorization techniques (e.g., QR decomposition) can 
be as computationally ineffective as the UKF since the 
complexity of QR is 0(n 3 ) (n denotes number of targets). 
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Fig. 3. Triangular factorization (a) Example X matrix (b) Example 
Y matrix 

As shown in Fig. [T] we have noted that the matrices in 
(TfOl i are nicely structured. Fig. |3a) illustrates an example 
schematically with two tracks, two registration parameters, 
and six measurements. The non-zero elements of the matrix 
in (fTTTb are denoted by crosses; and a blank position represents 
a zero element. 

Givens rotation ifTOl is used to eliminate the non-zero 
elements of the matrix C x in J10I . shown in Fig. [3a) as 
crosses surrounded by circles. Givens rotation is applied from 



the left to the right and for each column from the top to the 
bottom. Each non-zero low-triangular element in the ;-th row 
of C x , is combined with the diagonal element in the same 
column in the matrix block R x to construct the rotation. If 
the element in C x is zero, then no rotation is needed. 

Consequently, the algorithm to factorize X can be written 
as Algorithm 2. Note that Algorithm 2 is an in-place algo- 
rithm, which uses a small, constant amount of extra storage 
space. 

Algorithm 2: Factorization of X 
Require: Given X defined in JlOt . 

Ensure: Output an upper triangular matrix X 1 — TX such 
that X n X' = X*X, with f an orthogonal matrix. 

1: Let L be the dimension of s. 

2: for all Column j in C x do 
3: for all Row i in C x do 

4: if C x (i,j) ^0 then 
5: Let a = R x (i,i) and f3 — C x (i,j). 

6: Construct the rotation Rot(a, /?) = 

a ' r , M, r 1 with r = ^FTW- 

—p/r a/r v ^ 

for all k such that j < k < N, with denoting 
the last column of X do 

if X(i, k) £ or X(i + L, k) =£ then 

X(i,k) 
X(i + L,k) 

10: Apply the rotation Rot(a, /3) to d, i.e., d! 

Rot(a,j3)d. 

11: Save d! back to X in the same location as 

12: end if 

13: end for 

14: end if 
15: end for 
16: end for 

17: Apply QR decomposition to the sub-matrix A 

Ra Za 
C a O — Ul 

the orthogonal and upper triangular matrices, respectively. 
Write the result U back to X in the same location as A 



Let d = 



A = T qr U with T qr and U being 



Lemma 2.1: If R x in dTOb is a block-diagonal matrix, then 
1) the result R x in (fTTT i has the same form as R x ; and 2) the 
complexity of Algorithm 2 is 0(mk 2 ), where m and k denote 
the numbers of measurements and sensors, respectively. 

Proof: The triangulation described in Algorithm 2 com- 
prises of two steps: first turn C x into a zero matrix us- 
ing Givens rotation; and second apply QR decomposition 
to the sub-matrix A. Note that C x in X is sparse. This 
is seen by expanding the measurement equation (O as 

= h(xi,dj) + vj where o^} denotes the measurements 
associated with the target track Xi from the j-th sensor; and 
m is the measurement association variable for the j-th target 
track. Therefore, there is at most 0(N X + kN a ) nonzero 
entries in each paired set of rows of R x and C x , where N x 
denotes the number of parameters for each tracked target; and 



N a denotes the number of registration parameters for each 
sensor. Since an off-diagonal zero entry <r in R x pairs up with 
a zero entry in C x in the rotation operations, the condition 
specified in Step 8 of Algorithm 2 is not satisfied, and then 
the off-diagonal entry ? remains zero after the rotation. Thus 
1) is established. 

We note that in each paired set of rows of R x and C x , 
there are at most 0(N X + kN a ) non-zero pairs. There- 
fore, in order to turn an element of C x to zero, at most 
0(N x + kN a ) rotations in Step 10 are needed. Providing that 
m measurements are presented, 0(mN x ) elements need to 
be eliminated to zero. Givens rotation is applied a maximum 
0(mN 2 + mkN x N a ) times to transform C x into a zero ma- 
trix, which is equivalent to 0(mk) additive and multiplicative 
operations. In the second step, additional 0(mk 2 ) operations 
are needed. Therefore, the total complexity is 0(mk 2 ). ■ 

Triangulation of ( fT~9b can be treated similarly as that of 
(fTTl . Fig. Ob) illustrates an example schematically with 
two tracks and two registration parameters. The non-zero 
elements of the matrix Y are denoted by crosses; and blank 
position represents a zero element. Givens rotation is used 
to eliminate the non-zero entries in the matrix R xG = 
—R X & X 1 G X , shown in Fig. [2b) as crosses annotated by 
circles. Givens rotation is applied from the left to the right 
and for each column from the top to the bottom. Each non- 
zero entry in the z-th row of the matrix R xG is paired with the 
element in the same column in R w to construct the rotation. 

Similarly, the algorithm to factorize Y is given as Algo- 
rithm 3. 

Lemma 2.2: If R x in ( TT~8b is a block-diagonal matrix, then 

1) the result R x (t + 1) in ( TT9b has the same form as R x \ and 

2) the complexity of Algorithm 3 is 0(nk), with n denoting 
the number of targets. 

Proof: Each individual target has its own system dy- 
namics equation. For the j-th target, ([ToT i can be expressed 
as: 

Xi{t + 1) = <!>ix(t) + GiWi(t) + u 2i 

where $;and Gi are Jacobian matrices; the nonlinear term 
U2i = f(x*,w*) — $iX*—GiW*; and w l (t) is represented by 
the information array [R w i,z W i\. Therefore, the correspond- 
ing collective quantities Q x , G x , and R w are in the same 
block-diagonal form. In addition, R^' 1 and R xG are in 
the same block-diagram form. Since each off-diagonal zero 
entry q in i?^" 1 pairs up with a zero entry in the rotation 
operations, the condition in Steps 8 and 24 is not satisfied 
for the pairs and, then, the off-diagonal entry q remains zero. 
Thus 1) is established. 

As Fig. [5Jb) shows, in the paired rows of R u , and R xG , 
there are at most 0(2N X + kN a ) non-zero pairs. Thus, to 
eliminate an element to zero, a maximum 0(2N x + kN a ) ro- 
tations in Steps 10 and 26 are needed. If « tracked targets are 
considered, 0(nN x ) elements need to be eliminated to zero. 
Givens rotation is applied a maximum 0(2jiN x + knN a N x ) 



Algorithm 3: Factorization of Y 
Require: Given Y defined in J X 8 b - 

Ensure: Output an upper triangular matrix Y 1 = TY such that 
yityi _ Y t Y, with T an orthogonal transformation. 

— Rx^ G x . Let L be the number of rows in 



where Xi denotes the i-th target. One can verify that 
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Let Ri G 

Rw 

for all Column j in R^ G do 
for all Row i in R xG do 

if Ri G (i,j)^0then 

Let a = Y{i,i) and /3 = R d xG (i,j). 

Construct the rotation Rot (a, f3). 

for all k such that j < k < N, with N denoting 

the last column of Y do 

if B(i, k) or B(i + L, k) then 
B(i,fc) 
S(i + L,fe) 
Apply the rotation Rot(a, /3) to d, i.e., d' = 
Rot(a,/?)d 

Save d! back to y in the same location as d. 
end if 
end for 
end if 
end for 
end for 

Let R d x = R x ^ 
Rw 

for all Column j in R d do 
for all Row i in R x do 

if Ri{i, j) / then 

Let a = + M) and /3 = iif (i, j 
Construct the rotation Rot(a,/3). 
for all k such that j + M < k < 
denoting the last column of Y do 
if B(i,k) / or B(i + L,k) ± 
B(i,k) 
B(i + L,k) 
Apply the rotation Rot(a, /3) to d, i.e., d! — 
Rot(a, P)d 

Save d! back to F in the same location as d. 
end if 
end for 
end if 
end for 
end for 



Let M be the number of columns of 



N, with N 
then 



Let d 



times to transform the matrix Y into a triangular matrix, 
which is equivalent to 0(nk) additive and multiplicative 
operations. 2) is established. ■ 
Lemma 2.3: The complexity of the back substitution op- 
eration expressed in (fT5t is 0(n + k 3 ). 

Proof: Since R x is a block-diagonal matrix, (fl3T l can be 
written as 



' R X1 ... 
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... R Xn 


Rx n a 
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Ra _ 
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a = R- 1 
X i = R Xi(Zxi ~ Rx ta a) 



(22) 



0((kN a ) 3 ) and 0(nN 3 ) operations are needed to solve a 
and X{ for i = 1, ...,n in (l22l . respectively. Let P Xi and P a 
be covariance matrices of Xi and a. We can write 



(23) 



Pa = R a 

P'Xi = R Xi R-Xi P-a PxiaPaRxiaR-a 

Additional 0((kN a ) 3 ) and 0(nN 3 ) operations are needed 
to solve P a and P Xi for i = 1, n in d23l . respectively. 
Therefore, the total complexity is 0(n + k 3 ). ■ 
To summarize, we establish the following proposition: 
Proposition 2.2: The complexity of the FMAP algorithm 
is 0((m + n)k 2 + k 3 ), where m, n and k denote the numbers 
of measurement equations, targets and sensors. 

Proof: The FMAP algorithm comprises of three ma- 
trix operations whose complexity are specified by Lemmas 
12.11 12.21 and 12.31 respectively. Thus the total complexity is 
0(mk 2 + nk + n + k 3 ) = 0((m + n)k 2 + k 3 ). ■ 
Note that the number of sensors k is a constant. The 
complexity in the above proposition is thus simplified as 
0(n+m). This shows FMAP scales linearly with the number 
of measurements and with the number of target tracks. 

G. Handling Changes of the State Vector 

So far, we have assumed the state vector s = [x, a] has a 
fixed dimension. This means that we have the same number 
of targets in the whole observation span. However this is 
rarely true in practice. The dimension of s changes due to 
new targets coming into or existing targets leaving the field- 
of-view of the sensors. 

Once s has been estimated at time instant t (c.f., Sec- 
tion III-BI ), the prior distribution of s(t + 1) (inferred from 
s) can be easily obtained using the method described in 
Section III-CI Therefore, the main problem is to compute the 
distribution of the updated state vector s'(t + 1) after changes 
of the state vector s. 

In the following we define different vectors: 

• x r : all targets that are visible at instant t and remain at 
instant t + 1; 

• x c : all new targets that are visible at instant t + 1 but 
not visible at instant t; 

> x d : all deleted targets that are visible at instant t but not 
visible at instant t + 1; 

• s r : the concatenated vector of the remaining targets and 
registration parameters, i.e., s r = [x r ,a\. 

Note that the following relationships hold: 



s{t + 1) 



x 
s r 



,*'(* + !) = 



x 
s r 



So we can find a permutation matrix ifTOl Pn = [P^\ Pjp] 
such that 



Pns(t + 1) 



Pi 



n s (t 

(2 
J 



1) 



X 

x r 
a(t + l) 



We can verify that the marginal distribution of s r as (c.f., 
Lemma 16.31 in Appendix A) 

pr pr ~r 

^x ^xa x 

R a {t + l) z a (t + l) 



with 



x r 




_ a{t+l) _ 




^ = 


p(2) 


R xa = 


p(2) 


^x = 


p(2) 

mi 



>(2) 



i?,(i+l)(P ] 
+ 1) 

where R x (t + 1), iL Q (i + 1), R a {t + 1), ^(t + 1), and 
z a (t + 1) are defined in J20t , We note that i?£ is a block- 
diagonal matrix, i.e., R x = diag([R,Yi d+1 , Rn n ]) where 
i?n t denotes the Ilj-th diagonal block in R x (t + 1). 

Giving the new targets x° = [x1,...,xi] at instant 
t + 1, we can write the updated state vector s'(i + 1) = 



"li x k 



Assume the new targets 



are distributed as x c ~ [i? c ,z c ] where i? c denotes a block- 
diagonal noninformative information matrix^. In addition we 
assume x c be statistically independent of s r . Therefore, the 
updated state vector s'(t + 1) is distributed as 



s'(t + l) 



R c z c 

R x Rxa Z T 

R a {t + 1) 2 (t + l) 



(24) 



III. AN ILLUSTRATIVE EXAMPLE 

The schematic illustration in Fig. |4] includes the sensors 
mounted at the front of a vehicle at positions A and B. A 
single target T, in front and in the same lane as the vehicle, 
moves away from the vehicle. 

In the scenario illustrated in Fig. [4] the positions of the 
sensors A and B are denoted by (£ao,Vao) and (^bo:Vbo)^ 
respectively. The orientations of the sensors A and Sensor 
B are denoted by ^>ao and ^bq, respectively. The target is 
located at position x = (£, ,77, ?;,,) in the ^77-coordinate 
system. Let the registration parameters of Sensor A and 



Fig. 4. An example 

B be a A = (£ A0 , rj A o, ^ao) and a B = (£bo, Vbo, *bo). 
respectively. Then the joint state vector s = [x a A as ]■ 
Providing each sensor measures range (r), range rate 
(f), and azimuth angle (9), the measurement functions 
h A = (r A ,f A ,9 A ) for Sensor A can be expressed as, 

r A = v(C - 6to) + (V ~ VA0) 2 , r A = vJn A and 6 A = 
arctan( ^Zj^° ) ~ ^ao, respectively, where v r and n A denote 
the relative velocity vector v n ) and unit vector along the 
direction from the target to Sensor A. Approximating the 
measurement functions h A in linear form at the neighborhood 
of the point (x*,a A ), produces: 



C Ax 



c)x 



£*-e*4< 



''.HI 








a 



dh I* 
da A 



" "."AO 
T A 



-1 



where x* = (^* ,v^,r}* ,v*), a* A = (^ ao ,V A o^ao)' and 

r* A = VW* Go) 2 + W ~ Vlv) 2 - 

Similarly, hs, Cbx, and cs a of the sensor B can be 
derived. 

Let the measurement vector o be denoted as 



O = (tAi '"Aj @Aj r B 1 Tb ; @b)^ 



(25) 



The matrix coefficients C x and C a of the measurement 
equation in (|6]i for the scenario illustrated in Fig. |4]can then 
be expressed as: 



C x 



Cax 
Cbx 



C a 



CAa 







CBa 



(26) 



(27) 



Assuming the system dynamics can be modeled with a 
constant velocity (CV) model, $ x , G x , 112 in ( fTSl can be 
written as 

1 AT 
10 
1 AT 
1 



(28) 



1 R c = diag(e/, el) and z c = [0, ...,0] T where e denotes a small 
positive number. 



Gx — I4 



U 2 = 



(29) 



Process noise w in ( [Tol l is denoted by the information array 
[R w , z w ), expressed as z w = and 



where W 



AT 3 
3 



^ ^ 



q( W 
q v W 




and 



9i 



(30) 



are random 



walking parameters for ^-coordinate and ry-coordinate, re- 
spectively. 

IV. Simulation 

As shown in Fig. |4] the simulation results presented here 
are based on two sensors simulated near the front bumper. 
Sensor A is located at (£ao = 2, t]ao = 0.6), oriented 10° 
{^ao = 10°) outwards from the vehicle's bore-sight (in- 
coordinate). Sensor B is located at (£bo = 2, i]bo = —0.6), 
oriented 10° (^bo = —10°) outwards from the vehicle's 
bore-sight. The random walking parameters in (l30l are ex- 
pressed as = 0.1 and q n = 0.1. The measurement noise 
variance (oy, a rr , ag) is set to (0.1, 0.2, 1°). Three algorithms 
are implemented: SEP, UKF, and FMAP. SEP (23 treats the 
tracking and registration problems separately; but the UKF 
and FMAP algorithms address the problems jointly. Each 
algorithm has been implemented in Matlab on a 2 GHz Intel 
Core 2 Duo processor running Window XP. No special care 
has been taken to produce efficient code. 

In the first simulation, ten targets are randomly generated 
in the field-of-view of the sensors over a period of about 
50 seconds. We initially set the registration parameters of 
the sensors randomly with small numbers. Tracks are ini- 
tialized using the zero-mean noninformative distributions. 
Fig. [5] shows the trajectories of unregistered measurements 
and an tracked target vs. ground truth. The red solid line 
represents the true trajectory of the target while the other 
blue solid, green dash-dotted, and dashed lines represent 
the fused results of the SEP, UKF, and FMAP algorithms, 
respectively. Fig. [6] show the mean errors^] of position and 
velocity estimates of the target for the three algorithms, 
respectively. The bar plot in Fig. [7] shows the mean errors 
of the registration estimates for the three algorithms. The 
performance difference between UKF and FMAP are too 
small to seen on the scale used in the Figures. Note that 
in all the cases the performance of FMAP and UKF is 
clearly superior to that of SEP. This confirms the asymptotical 
optimality of JTR algorithm versus the decoupled approaches 
(e.g., SEP algorithm). 

In the next simulation, we investigate the time complexity 
of the SEP, UKF, and FMAP algorithms by varying the 

8 The mean error is defined as e x = E{\x — xt\} with x and xt the 
measurement and ground truth, respectively. Giving a series of measurement 
and truth pairs (xi,xxi), I < i < N, the sample mean error is computed 

£f \xi-x Ti \ 
n« /-j — ±±1 — ! — - ±-LL 



number of simulated targets from 10 to 300. Fig. |8]plots the 
execution time curves for the three algorithms at the different 
input sizes. The figure clearly illustrates the factor that the 
time complexities of FMAP and SEP are similar and are both 
an order of magnitude lower than that of UKF. This confirms 
the superior time-efficiency of FMAP vis-a-vis that of UKF. 
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Fig. 5. Top-down view of the trajectories of registered (left) and 
unregistered (right) sensor measurements of an tracked target vs. 
ground truth, respectively 
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Fig. 6. Mean errors of estimated longitudinal and lateral positions 
(e 5 and e v ) and the corresponding velocities (e^,e^) for a tracked 
target. 

The last simulation investigates how FMAP responses in 
the step change of registration. For example, in Fig. [9] the 
orientation of Sensor B (^bo) steps from —10° to —5° at 
Second 25 (see the third plots from the top), namely, the 
sensor rotates counterclockwise 5° that may be caused by an 
external collision impact. As in the first simulation, ten targets 
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Fig. 7. Mean errors of sensor registration estimates for Sensor B. 
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are randomly generated for the 50-second simulation. The 
innovation error curve has two peaks caused by two different 
cases: unknown registration at Second and step change of 
registration at Second 25, respectively. As shown in Fig. |9] 
the registration estimates converge to their corresponding 
ground truth within 5 seconds for both cases. 

V. Vehicular Experiment 

A test vehicle (Fig. [10] (a)) equipped with two SICK 
LMS291 rangefinders is used as the test-bed to verify the 
effectiveness of the proposed FMAP. The two rangefind- 
ers are mounted at two corners of the front bumper. The 
positions and orientations of the rangefinders are precisely 
surveyed. The left rangefinder (Sensor A) is located at 
(Uo = 3.724,7^0 = 0.883), oriented 45° (* A o = 45°) out- 
wards from the vehicle's bore-sight (^-coordinate). The right 
rangefinder (Sensor B) is located at (£bq = 3.720, t?bo = 
-0.874), oriented 45° (* B0 = -45°) outwards from the 
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Fig. 9. The response under step change of sensor registration. The 
top three plots are for the estimates of the registration parameters 
£bo> Vbo, and ^so vs - ground truth. The bottom plot is the 
innovation error. 



vehicle's bore-sight. The vehicle is also equipped with wheel 
encoders and an IMU sensor for determining the motion 
control inputs. 

The rangefinder scans from right to left in its 180° field- 
of-view at a resolution of 0.5° and generates 361 distance 
measurements to the closest line-of-sight obstacles. Each 
rangefinder is equipped with a point-object detector (e.g., 
tree trunks, light poles, and etc.) that selects scan clusters 
such that 1) the size of the cluster is less than 0.5 meter, 
and 2) the distance to the nearest point of other clusters is 
larger than 5 meters. The speed (i.e., range rate) of a point 
object is inferred from the host vehicle motion because of 
the stationary assumption of the point objects. 

The experiment □ was conducted in a parking lot with 
plenty road-side point objects shown in Fig. [10] (b). The 
vehicle was manually driven, and an observation sequence 
of about 50 seconds was collected. The plot (c) shows a 
snapshot of unregistered range data for the scene in Fig. [10] 
(b). The black dots and magenta circles in the plot denote the 
scan data generated by the left and right rangefinders, respec- 
tively. The blue circles and red stars denote the detected point 
objects from the left and right rangefinders, respectively. 

The measurement inconsistence caused by the registration 
bias is clearly seen. For example, the contours of the vehicle 
and the point objects in (b) from the sensors is not aligned. 
The inconsistence raises an issue for data association. With- 
out a consistent registration, data from one sensor cannot be 
correctly associated with that from a different sensor. For 
comparison, the snapshot of the registered range data of the 
same scene using FMAP algorithm is shown in Fig. [10] (d). 

9 The demo source code can be downloaded from 
http://code.google.eom/p/joint-calibration-tracking/ along with the data 
set. 




The additional black diamonds in (d) denote the fused targets. 
This plot clearly reveals the fact that FMAP significantly 
improves the sensor registration and thus the data association 
among multiple sensors. 

In each discrete time instant, the detected objects from both 
sensors are matched with the tracked targets (drawn as black 
diamonds in Fig. [10] (d)) at previous instant using a nearest- 
neighbor and a maximum distance gating data association 
strategy. We then apply the FMAP filter to jointly track 
the associated track-object pairs and registration parameters. 
Note that no target is visible in the whole observation 
sequence. As shown in the last plot in Fig.QjT. the dimension 
of the state vector s changes due to new targets coming into 
or existing targets leaving the field-of-view of the sensors. 
The method described in Section III-GI is used to handle the 
change. 

In the experiment, the right rangefinder's registration 
is unknown and initially set to (£bo = 2.720, i]bo = 
-0.126, * B0 = -40°). The SEP and FMAP algorithms are 
applied to the data set. The resulted registration error curves 
for both algorithms versus the surveyed values are shown 
in Fig. [TTT a). The blue solid lines and the magenta dashed 
lines denote the error curves of SEP and FMAP, respectively. 
FMAP clearly exhibits superior performance comparing with 
that of SEP. A shown in Fig. [TTTa), the estimates of the 
sensor's position (£bo, Vbo) and orientation ^bo converge to 
their true values in about 5 seconds, respectively. The slight 
transitory oscillation near the start of the run is partially due 
to the vehicle's steering maneuver and lack of shared detected 
objects from both sensors. Selecting a suitable model for the 
target motion requires giving consideration to the tradeoff 
between the filter accuracy and the model complexity. Better 
results can be obtained by incorporating vehicular motion 
model and global target map building. This may increase the 
computational complexity of the algorithm. The fourth plot 
in [TTT a) shows the number targets tracked by FMAP. 

On the other hand, the significant mean registration error 
by the SEP algorithm can be observed in Fig. [TTT b). We 
should note that the divergence of SEP curves increases over 
time in Fig. [TTT a). and this is partially contributed by the 
erroneous data association caused by registration biases of 
previous cycles. Therefore, the estimates of registration by 
FMAP is more consistent and robust than that by SEP. 

VI. SUMMARY AND CONCLUSION 

In this article, we have addressed the recursive JTR. The 
FMAP algorithm is derived in which the time complexity 
scales linearly with the numbers of measurements and targets. 
It is proved that FMAP is asymptotically optimal and has an 
0(n) implementation based on matrix orthogonal factoriza- 
tion. The results from experiments on synthetic and vehic- 
ular data demonstrate that, as expected, FMAP consistently 
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Fig. 10. (a) The test-bed vehicle equipped with two rangefinders. (b) 
The snapshot of a scene in the parking lot. (c) The top-down view 
of unregistered range data of the scene in (b). (d) The top-down 
view of registered range data of the scene in (b). 



performs better than the methods where tracking and regis- 
tration are separately treated. It has been also demonstrated 
experimentally using the synthetic data that the complexity 
of FMAP is indeed 0(n). Although FMAP and UKF are 
quite equivalent in performance but the time complexity of 
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Fig. 11. (a) The error curves of sensor registration estimates (£bo> 
rjBo and ^bo) for the right rangefinder when the true value is 
initially unknown. The last plot shows the number of detected 
objects varies with time, (b) The corresponding mean errors for 
the registration estimates. 

FMAP is a magnitude lower than that of UKF. Additionally, 
results of simulation and vehicle experiment demonstrate that 
FMAP can handle the dynamics of sensor registration and a 
variable number of targets. 
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Appendix A 



Using Lemma [67X1 we obtain the 



Lemma 6.1: Let p ~ A/"(yU p ,S p ) or in information array 
terms, {R pi z p \. If u = ap + p, u ~ N{ap p + P, aE p a*), 
or in information array terms, cj can be represented through 
[iio,, Zu], where # w = ^a" 1 and z u = z p + 

Proof: Since /i p = Rp~ l z p and £ p = R~ 1 R~ t , we get 



"M P + /? 



and 



and 





-R x 3>~ G x Rx&x 1 Rxa 
R a 



z p + (R p a 1 )/3 = 



2«, 



E w = flj 1 ^* = aS p a* = aR- 1 R p t a t 

One can verify that i?^ = R p a~ l and 

2 U = = Ruiiap-p + P) = RwiaR^Zp + P) 

= R p or 1 {aR- x z p + p) 
= (Rpa-^iaR-^Zp + iRpa- 1 )? 
= z p + {RpOT^P 
= z p + Ru/3 

■ 

Lemma 6.2: Let the system dynamics be defined in < TT~6b : 
the density function of the state variable s(t) = [x, a] be ex- 
pressed as information array in (03); and random noise term 
w be in information array form [Rw, z w ]. Let p = [w, s(t)]'. 
If w and s(t) are statistically independent, the joint density 
of oj = [w, x(t + 1), a(t + 1)]* can be represented through 
the information array \R w ,z w ], where 



Ru 



R w 

-R x <b~ l G x Rx&x 1 Rxa 

R a 



and 



z x + R x <$> x 1 u 2 

Za 

Proof: Since w and s(t) are statistically indepen- 
dent, the information array of the joint vector p = 

[w,s(t)Y 



Ra 





[to, 


x, a]* 


R w 











Rx 










Ra 



and z p = [ 



One verifies that the system dynamics equation dT6b can 
be reorganized as 



and a 1 = 





UJ = 


- ap + P 






'I 0" 







with a 


G x $ x 


and p 


1*2 







I 







1 


" 



















I 









Lemma 6.3: Let p 

'(av s p) 



Pi 

AF(Hp,Hp) or information array [i? p ,z p ] with i? p 



"•pii 




i? 



"-P12 
P22 



and z 



"pi 



be distributed as p ~ 
,z p ] with i? p = 
Then the marginal 



distribution of p 2 is normal and has p 2 ~ [-R P22 > z p 2 

-^pii ~Rp 11 Rp\?Rpii 
i?" 1 



mean 



/'/- 



^pil 



R- 1 



P2 2 



R P22 



Thus the 









22 



Pll^Pl Rp 11 Rpi2Rp 22 Z ' 2 



L P22 ^P2 

The marginal distribution of p2 ~ N(n P2 , £ P2 ) with /x P2 
- R p~X 2 - Therefore p 2 ~ [i^,*, 



^p 22 Z P2 aIld E P2 



